import math
import time
import numpy as np
import matplotlib.pyplot as plt


def calculate_distance(point1, point2):
    # 转换为 NumPy 数组
    point1 = np.array(point1)
    point2 = np.array(point2)

    # 计算坐标差的平方
    diff = point2 - point1
    squared_diff = np.square(diff)

    # 求和并取平方根
    distance = np.sqrt(np.sum(squared_diff))

    return distance


def calculate_point_on_vector(direction_vector, point, distance):
    # direction_vector = np.array([x2 - x1, y2 - y1, z2 - z1])  # 方向向量
    length = np.linalg.norm(direction_vector)  # 方向向量的长度
    scaled_vector = (distance / length) * direction_vector  # 缩放后的方向向量
    return np.array([point[0] - scaled_vector[0], point[1] - scaled_vector[1], point[2] - scaled_vector[2]])  # 计算目标点坐标


# vector 为要旋转的向量
# axis 旋转轴向量
# angle 旋转角度（弧度）
def rotate_vector_around_axis(vector, axis, angle):
    # 归一化向量
    vector = vector / np.linalg.norm(vector)
    axis = axis / np.linalg.norm(axis)

    # 计算旋转四元数
    q = quaternion_from_axis_angle(axis, angle)

    # 执行四元数旋转
    rotated_vector = quaternion_rotate_vector(q, vector)

    return rotated_vector


def quaternion_from_axis_angle(axis, angle):
    # 计算旋转四元数
    angle /= 2
    axis = axis / np.linalg.norm(axis)
    q = np.array([np.cos(angle), axis[0] * np.sin(angle), axis[1] * np.sin(angle), axis[2] * np.sin(angle)])

    return q


def quaternion_rotate_vector(q, v):
    # 执行四元数旋转
    q_conj = np.array([q[0], -q[1], -q[2], -q[3]])
    rotated_vector = quaternion_multiply(quaternion_multiply(q, np.concatenate(([0], v))), q_conj)[1:]

    return rotated_vector


def quaternion_multiply(q1, q2):
    # 四元数乘法
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
    z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2

    return np.array([w, x, y, z])


def calculate_vertex(normal, center, pipe_shape, pipe_width, pipe_height, num_interpolation_points):
    distance = 0
    angle_array = []
    if pipe_shape == 1:  # 圆管
        # num_interpolation_points = 12
        distance = pipe_width
        # angle_array = [0, np.pi / 6, np.pi / 3, np.pi / 2, 2 * np.pi / 3, 5 * np.pi / 6, np.pi, 7 * np.pi / 6,
        #                4 * np.pi / 3, 3 * np.pi / 2, 5 * np.pi / 3, 11 * np.pi / 6]
        for i in range(num_interpolation_points):
            angle_array.append(2 * i * np.pi / num_interpolation_points)
    else:  # 方管
        distance = math.sqrt(math.pow(pipe_width / 2, 2) + math.pow(pipe_height / 2, 2))
        angle_tmp = math.atan(pipe_width / pipe_height)
        angle_array = [angle_tmp, np.pi - angle_tmp, np.pi + angle_tmp, 2 * np.pi - angle_tmp]

    # 旋转轴为 normal
    # 首先计算要旋转的向量
    v1 = np.cross(normal, np.array([0, 0, 1]))
    vector_rotate = np.cross(normal, v1)

    result_array = []
    for angle in angle_array:
        vector_tmp = rotate_vector_around_axis(vector_rotate, normal, angle)
        u_tmp = vector_tmp / np.linalg.norm(vector_tmp)
        pt = center + u_tmp * distance
        result_array.append((round(pt[0], 3), round(pt[1], 3), round(pt[2], 3)))

    return result_array


def divide_arc(center, start_point, end_point, n):
    v1 = start_point - center
    v2 = end_point - center

    r = np.linalg.norm(v1)

    u1 = v1 / np.linalg.norm(v1)
    u2 = v2 / np.linalg.norm(v2)

    theta = np.arccos(np.dot(u1, u2))

    rotation_axis = np.cross(u1, u2)
    rotation_axis /= np.linalg.norm(rotation_axis)

    rotation_matrix = rotation_matrix_from_axis_angle(rotation_axis, theta)

    points = []
    for i in range(n):
        angle = i * theta / (n - 1)
        rotated_point = rotate_around_axis(start_point, center, rotation_axis, angle)
        points.append(rotated_point.tolist())

    return points


def rotation_matrix_from_axis_angle(axis, angle):
    axis = np.asarray(axis)
    axis /= np.linalg.norm(axis)
    a = np.cos(angle / 2.0)
    b, c, d = -axis * np.sin(angle / 2.0)
    rotation_matrix = np.array([[a * a + b * b - c * c - d * d, 2 * (b * c - a * d), 2 * (b * d + a * c)],
                                [2 * (b * c + a * d), a * a + c * c - b * b - d * d, 2 * (c * d - a * b)],
                                [2 * (b * d - a * c), 2 * (c * d + a * b), a * a + d * d - b * b - c * c]])
    return rotation_matrix


def rotate_around_axis(point, axis_origin, axis_direction, angle):
    axis_origin = np.asarray(axis_origin)
    axis_direction = np.asarray(axis_direction)
    point = np.asarray(point)

    v = point - axis_origin
    rotated_v = v * np.cos(angle) + np.cross(axis_direction, v) * np.sin(angle) + axis_direction * np.dot(
        axis_direction, v) * (1 - np.cos(angle))
    rotated_point = rotated_v + axis_origin

    return rotated_point


def find_intersection(line1_point, line1_direction, line2_point, line2_direction):
    A = np.vstack((line1_direction, -line2_direction)).T
    b = line2_point - line1_point

    try:
        t, s = np.linalg.lstsq(A, b, rcond=None)[0]
    except np.linalg.LinAlgError:
        return None

    intersection_point = line1_point + t * line1_direction
    # intersection_point = line2_point + s * line2_direction
    return intersection_point


# 在直线1，直线2的交点处计算相关参数
def calculate_pipe_params(start_point_line1, end_point_line1, start_point_line2, end_point_line2, turning_radius,
                          num_segments_on_arc, num_segments_per_circle_section, pipe_shape, pipe_width_line1,
                          pipe_height_line1, pipe_width_line2,
                          pipe_height_line2):
    vector_line1 = end_point_line1 - start_point_line1
    vector_line2 = start_point_line2 - end_point_line2
    tmp_end_point_line1 = calculate_point_on_vector(vector_line1, end_point_line1, turning_radius)
    tmp_start_point_line2 = calculate_point_on_vector(vector_line2, start_point_line2, turning_radius)
    plane_normal = np.cross(vector_line1, vector_line2)
    vector_line1_vertical = np.cross(plane_normal, vector_line1)
    vector_line2_vertical = np.cross(plane_normal, vector_line2)
    circle_center = find_intersection(tmp_end_point_line1, vector_line1_vertical, tmp_start_point_line2,
                                      vector_line2_vertical)
    circle_radius = calculate_distance(circle_center, tmp_end_point_line1)
    arc_segments = divide_arc(circle_center, tmp_end_point_line1, tmp_start_point_line2, num_segments_on_arc)
    elbow_points_vertices = []

    for i in range(num_segments_on_arc):
        segment = arc_segments[i]
        tmp_pipe_width = 0
        tmp_pipe_height = 0
        if pipe_width_line1 == pipe_width_line2 and pipe_height_line1 == pipe_height_line2:
            tmp_pipe_width = pipe_width_line1
            tmp_pipe_height = pipe_height_line1
        else:
            tmp_pipe_width = pipe_width_line1 + i * (pipe_width_line2 - pipe_width_line1) / num_segments_on_arc
            tmp_pipe_height = pipe_height_line1 + i * (pipe_height_line2 - pipe_height_line1) / num_segments_on_arc
        tmp_vector_segment = np.cross(plane_normal, circle_center - segment)
        elbow_point_vertices = calculate_vertex(tmp_vector_segment, segment, pipe_shape, tmp_pipe_width,
                                                tmp_pipe_height, num_segments_per_circle_section)
        elbow_points_vertices.append(elbow_point_vertices)
    end_point_line1_vertices = calculate_vertex(vector_line1, tmp_end_point_line1, pipe_shape, pipe_width_line1,
                                                pipe_height_line1, num_segments_per_circle_section)
    start_point_line2_vertices = calculate_vertex(vector_line2, tmp_start_point_line2, pipe_shape, pipe_width_line2,
                                                  pipe_height_line2, num_segments_per_circle_section)
    # return arc_segments, start_point_line1, tmp_end_point_line1, tmp_start_point_line2, end_point_line2
    return elbow_points_vertices, end_point_line1_vertices, start_point_line2_vertices


if __name__ == "__main__":
    print("start")
    start_time = time.time()
    # 管线1 起点为[0, 0, 2]，终点为[10, 10, 2.5]
    # 管线2 起点为[10, 10, 2.5]，终点为[20, 10, 2.2]
    # 连接部位进入弧段的长度距离端点长度为 0.3
    # 按照10段进行切分
    turning_radius = 0.3
    num_segments_on_arc = 10
    pipe_shape = 2  # 圆形
    num_segments_per_section = (12 if pipe_shape == 1 else 4)
    A = np.array([0, 0, 2])  # Coordinates of point A
    B = np.array([10, 10, 2.5])  # Coordinates of point B
    C = np.array([10, 10, 2.5])  # Coordinates of point C
    D = np.array([20, 10, 2.2])  # Coordinates of point D
    # normal = B - A
    # center = B
    #
    # vertices = calculate_vertex(normal, center, 2, 0.3, 0.4)

    arc_segments = calculate_pipe_params(A, B, C, D, turning_radius, num_segments_on_arc,
                                         num_segments_per_section, pipe_shape, 0.3, 0.4, 0.3, 0.4)
    # for segment in arc_segments[0]:
    #     print(segment)

    vertices = np.concatenate(arc_segments[0])
    print(vertices)
    print(len(vertices))
    mesh = []
    faces = []
    for vertex in vertices:
        mesh.append((vertex[0], vertex[1], vertex[2]))

    for i in range(len(arc_segments[0])):
        if i == 0:  # 构建其实截面
            tmp_index = []
            for k in range(num_segments_per_section):
                tmp_index.append(k)
            faces.append(tmp_index)
        elif i > 0:
            for j in range(num_segments_per_section):
                if j < num_segments_per_section - 1:
                    faces.append([(i - 1) * num_segments_per_section + j,
                                  (i - 1) * num_segments_per_section + j + 1,
                                  i * num_segments_per_section + j + 1,
                                  i * num_segments_per_section + j])
                else:
                    faces.append([(i - 1) * num_segments_per_section + j,
                                  (i - 1) * num_segments_per_section,
                                  i * num_segments_per_section,
                                  i * num_segments_per_section + j])
            if i == len(arc_segments[0]) - 1:
                tmp_index = []
                for k in range(num_segments_per_section):
                    tmp_index.append(i*num_segments_per_section+k)
                faces.append(tmp_index)
    print(mesh)
    print(faces)
    end_time = time.time()
    execution_time = end_time - start_time
    print("代码执行时间：", execution_time, "秒")
    # # 可视化
    # fig = plt.figure()
    # ax = fig.add_subplot(111, projection='3d')
    # # ax.axis('equal')
    #
    # x = []
    # y = []
    # z = []
    #
    # for segment in arc_segments[0]:
    #     for vertex in segment:
    #         x.append(vertex[0])
    #         y.append(vertex[1])
    #         z.append(vertex[2])
    #
    # ax.scatter(x, y, z, c='b', marker='o')
    #
    # # 设置坐标轴标签
    # ax.set_xlabel('X')
    # ax.set_ylabel('Y')
    # ax.set_zlabel('Z')
    #
    # # 显示图形
    # plt.show()
